#!/usr/bin/env python
# -*- Python -*-

"""
 \file VisionManipulation.py
 \brief Vision Manipulation
 \date $Date$


"""
import sys
import time
sys.path.append(".")

# Import RTM module
import RTC
import OpenRTM_aist

# Import Service implementation class
# <rtc-template block="service_impl">
from VisionManipulation_idl_example import *

# </rtc-template>

# Import Service stub modules
# <rtc-template block="consumer_import">
import _GlobalIDL, _GlobalIDL__POA

# </rtc-template>


# This module's spesification
# <rtc-template block="module_spec">
visionmanipulation_spec = ["implementation_id", "VisionManipulation", 
		 "type_name",         "VisionManipulation", 
		 "description",       "Vision Manipulation", 
		 "version",           "1.0.0", 
		 "vendor",            "AIST", 
		 "category",          "VMRG", 
		 "activity_type",     "STATIC", 
		 "max_instance",      "0", 
		 "language",          "Python", 
		 "lang_type",         "SCRIPT",
		 ""]
# </rtc-template>

class RecogResult:
    def __init__(self, array):
        print "RecogResult.init: "
        print array
        if not array:
            return 
        
        if len(array) != 20:
            raise "arg must be an array contain 20 numerics."
        self.camera_id, self.object_id, self.candidate_num , self.coordinate_num = map(long, array[0:4])
        self.reliability, self.error_num = map(long, array[4:6])
        self.position = [array[11]/1000, array[15]/1000, array[19]/1000]
        self.orientation = array[8:11] + array[12:15] + array[16:19]
          
    def __str__(self):
        return "<RecogResult: camera_id(%d); object_id(%d); candidate_num(%d); coordinate_num(%d)\n" % self.camera_id, self.object_id, self.candidate_num , self.coordinate_num + \
          " reliability(%d); error_num(%d);\n" % self.reliability, self.error_num + \
          " [%f, %f, %f]\n" % self.orientation[0:3] + \
          " [%f, %f, %f]\n" % self.orientation[3:6] + \
          " [%f, %f, %f]\n" % self.orientation[6:9] + \
              " [%f, %f, %f]>\n\n" % self.position

              

class VisionManipulation(OpenRTM_aist.DataFlowComponentBase):
	
	"""
	\class VisionManipulation
	\brief Vision Manipulation
	
	"""
	def __init__(self, manager):
		"""
		\brief constructor
		\param manager Maneger Object
		"""
		OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)

		self._d_recogResultIn = RTC.TimedDoubleSeq(RTC.Time(0,0),[])
		"""
		"""
		self._recogResultIn = OpenRTM_aist.InPort("recogResult", self._d_recogResultIn)

		"""
		"""
		self._recogPortPort = OpenRTM_aist.CorbaPort("recogPort")
		"""
		"""
		self._SchedulerPort = OpenRTM_aist.CorbaPort("Scheduler")
		"""
		"""
		self._PlanStartPortPort = OpenRTM_aist.CorbaPort("PlanStartPort")
		"""
		"""
		self._recogSDLPort = OpenRTM_aist.CorbaPort("recogSDL")

		"""
		"""
		self._scheduler = SchedulerService_i()
		

		"""
		"""
		self._recogPort = OpenRTM_aist.CorbaConsumer(interfaceType=_GlobalIDL.RecognitionService)
		"""
		"""
		self._plan_start = OpenRTM_aist.CorbaConsumer(interfaceType=_GlobalIDL.GraspPlanStart)
		"""
		"""
		self._recogSDL = OpenRTM_aist.CorbaConsumer(interfaceType=_GlobalIDL.RecognitionServiceSDL)

		# initialize of configuration-data.
		# <rtc-template block="init_conf_param">
		
		# </rtc-template>


		 
	def onInitialize(self):
		"""
		
		The initialize action (on CREATED->ALIVE transition)
		formaer rtc_init_entry() 
		
		\return RTC::ReturnCode_t
		
		"""
		# Bind variables and configuration variable
		
		# Set InPort buffers
		self.addInPort("recogResult",self._recogResultIn)
		
		# Set OutPort buffers
		
		# Set service provider to Ports
		self._SchedulerPort.registerProvider("interface", "SchedulerService", self._scheduler)
		
		# Set service consumers to Ports
		self._recogPortPort.registerConsumer("recogPort", "RecognitionService", self._recogPort)
		self._PlanStartPortPort.registerConsumer("PlanStart", "GraspPlanStart", self._plan_start)
		self._recogSDLPort.registerConsumer("recogSDL", "RecognitionServiceSDL", self._recogSDL)
		
		# Set CORBA Service Ports
		self.addPort(self._recogPortPort)
		self.addPort(self._SchedulerPort)
		self.addPort(self._PlanStartPortPort)
		self.addPort(self._recogSDLPort)
		
		self._scheduler.recognition_service=self._recogPort		
		#self._scheduler.setRecogSDLService(self._recogSDL)

		return RTC.RTC_OK
	
	#def onFinalize(self, ec_id):
	#	"""
	#
	#	The finalize action (on ALIVE->END transition)
	#	formaer rtc_exiting_entry()
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onStartup(self, ec_id):
	#	"""
	#
	#	The startup action when ExecutionContext startup
	#	former rtc_starting_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onShutdown(self, ec_id):
	#	"""
	#
	#	The shutdown action when ExecutionContext stop
	#	former rtc_stopping_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	def onActivated(self, ec_id):
	#	"""
	#
	#	The activated action (Active state entry action)
	#	former rtc_active_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
		self._scheduler.busy = False	  	 	
		return RTC.RTC_OK
	
	#def onDeactivated(self, ec_id):
	#	"""
	#
	#	The deactivated action (Active state exit action)
	#	former rtc_active_exit()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK

	def onExecute(self, ec_id):
		"""
	
		The execution action that is invoked periodically
		former rtc_active_do()
	
		\param ec_id target ExecutionContext Id
	
		\return RTC::ReturnCode_t
	
		"""
		#try:
		if not self._recogResultIn.isNew():
			time.sleep(0.01)
			return RTC.RTC_OK

		print "resultIn:"
		indata = self._recogResultIn.read()
  		val = indata.data
  		print "read: len(%d)" % len(val)

  		if len(val) < 20:
			print "Too few Recognition Result." 
			self._scheduler.busy = False	  	 	
			return RTC.RTC_OK
		print val[0:20]
  		recog = RecogResult(val[0:20])
  		print "error: %d" % recog.error_num
		if recog.error_num != 0:
			print "Recognition Error: %d" % recog.error_num 
			self._scheduler.busy = False	  	 	
			return RTC.RTC_OK

  		if not self._plan_start._ptr():
  			print "GraspPlanStart interface is None."
  			self._scheduler.busy = False	  	 	
 			return RTC.RTC_OK
  			
  		print "GraspPlanningStart"
  		print "objID: %d" % recog.object_id
  		print "pos(%d)" % len(recog.position), recog.position
  		print "ori(%d)" % len(recog.orientation), recog.orientation
		state = self._plan_start._ptr().GraspPlanningStart(recog.object_id, recog.position, recog.orientation)
		print "state(%d)" % state

		self._scheduler.busy = False	  	 	
		#except:
	  	#	print "Exception cought in onExecute()"

		return RTC.RTC_OK
	
	#def onAborting(self, ec_id):
	#	"""
	#s
	#	The aborting action when main logic error occurred.
	#	former rtc_aborting_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onError(self, ec_id):
	#	"""
	#
	#	The error action in ERROR state
	#	former rtc_error_do()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onReset(self, ec_id):
	#	"""
	#
	#	The reset action that is invoked resetting
	#	This is same but different the former rtc_init_entry()
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onStateUpdate(self, ec_id):
	#	"""
	#
	#	The state update action that is invoked after onExecute() action
	#	no corresponding operation exists in OpenRTm-aist-0.2.0
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	
	#def onRateChanged(self, ec_id):
	#	"""
	#
	#	The action that is invoked when execution context's rate is changed
	#	no corresponding operation exists in OpenRTm-aist-0.2.0
	#
	#	\param ec_id target ExecutionContext Id
	#
	#	\return RTC::ReturnCode_t
	#
	#	"""
	#
	#	return RTC.RTC_OK
	



def VisionManipulationInit(manager):
    profile = OpenRTM_aist.Properties(defaults_str=visionmanipulation_spec)
    manager.registerFactory(profile,
                            VisionManipulation,
                            OpenRTM_aist.Delete)

def MyModuleInit(manager):
    VisionManipulationInit(manager)

    # Create a component
    comp = manager.createComponent("VisionManipulation")

def main():
	mgr = OpenRTM_aist.Manager.init(sys.argv)
	mgr.setModuleInitProc(MyModuleInit)
	mgr.activateManager()
	mgr.runManager()

if __name__ == "__main__":
	main()

